#include "motion_plan.h"
#include <math.h>
#include <iostream>
#include <algorithm>

// 地球半径 (米)
const double EARTH_RADIUS = 6378137.0;

// 度到弧度的转换
inline double deg2rad(double deg) {
    return deg * M_PI / 180.0;
}

// 弧度到度的转换
inline double rad2deg(double rad) {
    return rad * 180.0 / M_PI;
}

MotionPlan::MotionPlan()
{
    // 初始化跟踪参数
    memset(&dotTrackingParam, 0, sizeof(TrackingPlanParam));
    dotTrackingParam.TrackNum = 0;
    dotTrackingParam.eta = 0.8f;                // 跟踪增益
    dotTrackingParam.switchlen = 15.0f;         // 航点切换距离（米）   帅：问题：这里可能需要将距离缩短一点，实船这里如果是15米的话太远了
    dotTrackingParam.maxSpeed = 5.0f;           // 最大速度限制
    dotTrackingParam.setVel = 2.0f;             // 默认速度
    dotTrackingParam.minSpeedFactor = 0.2f;     // 最小速度因子
    
    // 初始化等待参数
    WaitTime = 0;
    WaitTimeNum = 0;
    WaitFlag = 0;
    
    // 初始化控制结构体
    memset(&tracking_task_, 0, sizeof(mission_control));
    memset(&actionModelData, 0, sizeof(flash_model_data));
    memset(&actionActuatorControl, 0, sizeof(actuator_control));
    memset(&actionHeadvelControl, 0, sizeof(headvel_control));
}

void MotionPlan::SetTrackingParameters(float eta, float switch_distance, float max_speed, float default_speed, float min_speed_factor)
{
    dotTrackingParam.eta = eta;
    dotTrackingParam.switchlen = switch_distance;
    dotTrackingParam.maxSpeed = max_speed;
    dotTrackingParam.setVel = default_speed;
    dotTrackingParam.minSpeedFactor = min_speed_factor;
}

bool MotionPlan::TrajectoryGet(const mission_control& target)
{
    // 重置跟踪参数
    ResetTracking();
    
    // 复制任务数据
    tracking_task_ = target;
    
    // 验证任务有效性
    if (tracking_task_.mission_num == 0 || tracking_task_.mission_num > 128) {
        std::cout << "Warning: Invalid mission points count: " << tracking_task_.mission_num << std::endl;
        return false;
    }
    
    // 设置跟踪参数
    dotTrackingParam.DotNum = tracking_task_.mission_num;
    dotTrackingParam.TrackNum = 0;
    dotTrackingParam.ReturnFlag = 0;
    dotTrackingParam.TrackFlag = 1;
    dotTrackingParam.FirstFlag = 1;
    
    // 设置初始目标
    dotTrackingParam.TargetPos[0] = tracking_task_.mission[0].lat;
    dotTrackingParam.TargetPos[1] = tracking_task_.mission[0].lon;
    
    // 使用param1作为速度参数，如果无效则使用默认速度
    float target_speed = tracking_task_.mission[0].param1;
    if (target_speed > 0 && target_speed <= dotTrackingParam.maxSpeed) {
        dotTrackingParam.setVel = target_speed;
    }
    
    // 计算轨迹单位向量（用于平滑跟踪）
    for (int i = 1; i < dotTrackingParam.DotNum; i++) {
        double bearing = calculateGeoBearing(
            tracking_task_.mission[i-1].lat, tracking_task_.mission[i-1].lon,
            tracking_task_.mission[i].lat, tracking_task_.mission[i].lon
        );
        
        // 转换为单位向量 (北东坐标系)
        dotTrackingParam.TrajectoryUnit[i-1][0] = static_cast<float>(sin(deg2rad(bearing))); // 东分量
        dotTrackingParam.TrajectoryUnit[i-1][1] = static_cast<float>(cos(deg2rad(bearing))); // 北分量
    }
    
    std::cout << "Trajectory initialized with " << dotTrackingParam.DotNum 
              << " waypoints, target speed: " << dotTrackingParam.setVel << " m/s" << std::endl;
    
    return true;
}

void MotionPlan::TrajectoryTrackingDirect(const TransState& boatstate)
{
    if (dotTrackingParam.ReturnFlag == 1) {
        // 任务已完成，发送停止指令
        actionModelData.model = 10;
        actionHeadvelControl.model = 2;
        actionHeadvelControl.heading = static_cast<float>(boatstate.PosEarth[3]); // 保持当前航向
        actionHeadvelControl.velocity = 0.0f;
        return;
    }
    
    if (dotTrackingParam.DotNum <= 0 || dotTrackingParam.TrackFlag == 0) {
        std::cout << "No active trajectory to track!" << std::endl;
        return;
    }
    
    // 检查是否到达当前航点
    if (checkWaypointReached(boatstate)) {
        advanceToNextWaypoint();
        if (dotTrackingParam.ReturnFlag == 1) {
            return;  // 任务完成
        }
        updateTargetPosition();
    }
    
    // 计算控制指令
    calculateControlCommand(boatstate);
}

void MotionPlan::calculateControlCommand(const TransState& boatstate)
{
    // 当前位置 (经纬度)
    double current_lat = boatstate.PosEarth[0];
    double current_lon = boatstate.PosEarth[1];
    // double current_heading = boatstate.PosEarth[3];
    
    // 目标位置 (经纬度)
    double target_lat = dotTrackingParam.TargetPos[0];
    double target_lon = dotTrackingParam.TargetPos[1];
    
    // 计算到目标的距离和方位角
    double distance_to_target = calculateGeoDistance(current_lat, current_lon, target_lat, target_lon);
    double bearing_to_target = calculateGeoBearing(current_lat, current_lon, target_lat, target_lon);
    
    // 期望航向角计算
    double desired_heading = bearing_to_target;
    
    // 如果不是第一个航点，应用轨迹单位向量进行路径跟踪
    if (dotTrackingParam.TrackNum > 0 && dotTrackingParam.TrackNum < dotTrackingParam.DotNum) {
        // 获取轨迹方向
        float track_east = dotTrackingParam.TrajectoryUnit[dotTrackingParam.TrackNum-1][0];
        float track_north = dotTrackingParam.TrajectoryUnit[dotTrackingParam.TrackNum-1][1];
        
        // 计算轨迹角度
        double track_angle = rad2deg(atan2(track_east, track_north));
        if (track_angle < 0) track_angle += 360.0;
        
        // 混合直接导航和轨迹跟踪
        double angle_diff = normalizeAngle(bearing_to_target - track_angle);
        
        // 根据距离调整混合比例
        double blend_factor = std::min(1.0, distance_to_target / (dotTrackingParam.switchlen * 2.0));
        desired_heading = track_angle + blend_factor * angle_diff;
    }
    
    // 归一化航向角到[0, 360)
    desired_heading = normalizeAngle(desired_heading);
    if (desired_heading < 0) desired_heading += 360.0;
    
    // 根据航向误差和距离调整速度
    // double heading_error = normalizeAngle(desired_heading - current_heading);
    float velocity = dotTrackingParam.setVel;
    
    // 转弯时减速 - 大角度误差时降低速度
    // double abs_heading_error = fabs(heading_error);
    // if (abs_heading_error > 30.0) {
        // double speed_factor = std::max(0.5, (180.0 - abs_heading_error) / 180.0);
        // velocity *= static_cast<float>(speed_factor);
    // }
    
    // 接近目标时减速
    // if (distance_to_target < dotTrackingParam.switchlen * 2.0) {
        // double distance_factor = std::max(static_cast<double>(dotTrackingParam.minSpeedFactor),
                                        // distance_to_target / (dotTrackingParam.switchlen * 2.0));
        // velocity *= static_cast<float>(distance_factor);
    // }
    
    // 确保速度在合理范围内
    velocity = std::max(velocity, dotTrackingParam.setVel * dotTrackingParam.minSpeedFactor);
    velocity = std::min(velocity, dotTrackingParam.maxSpeed);
    
    // 设置控制指令
    actionModelData.model = 10;
    actionModelData.flash_flag = 1;
    actionHeadvelControl.model = 2;  // 航向速度控制模式
    actionHeadvelControl.heading = static_cast<float>(desired_heading);
    actionHeadvelControl.velocity = velocity;
}

bool MotionPlan::checkWaypointReached(const TransState& boatstate)
{
    double distance = calculateGeoDistance(
        boatstate.PosEarth[0], boatstate.PosEarth[1],
        dotTrackingParam.TargetPos[0], dotTrackingParam.TargetPos[1]
    );
    return (distance <= dotTrackingParam.switchlen);
}

void MotionPlan::advanceToNextWaypoint()
{
    dotTrackingParam.TrackNum++;
    
    if (dotTrackingParam.TrackNum >= dotTrackingParam.DotNum) {
        // 所有航点已完成
        dotTrackingParam.ReturnFlag = 1;
        dotTrackingParam.TrackFlag = 0;
        std::cout << "All waypoints completed!" << std::endl;
    } else {
        // 更新目标速度
        float target_speed = tracking_task_.mission[dotTrackingParam.TrackNum].param1;
        if (target_speed > 0 && target_speed <= dotTrackingParam.maxSpeed) {
            dotTrackingParam.setVel = target_speed;
        }
        
        std::cout << "Advancing to waypoint " << dotTrackingParam.TrackNum + 1 
                  << "/" << dotTrackingParam.DotNum 
                  << ", speed: " << dotTrackingParam.setVel << " m/s" << std::endl;
    }
}

void MotionPlan::updateTargetPosition()
{
    if (dotTrackingParam.TrackNum < dotTrackingParam.DotNum) {
        dotTrackingParam.TargetPos[0] = tracking_task_.mission[dotTrackingParam.TrackNum].lat;
        dotTrackingParam.TargetPos[1] = tracking_task_.mission[dotTrackingParam.TrackNum].lon;
    }
}

// void MotionPlan::ResetTracking()
// {
//     memset(&dotTrackingParam, 0, sizeof(TrackingPlanParam));
//     dotTrackingParam.TrackNum = 0;
//     dotTrackingParam.eta = 0.8f;
//     dotTrackingParam.switchlen = 15.0f;
//     dotTrackingParam.maxSpeed = 5.0f;
//     dotTrackingParam.setVel = 2.0f;
//     dotTrackingParam.minSpeedFactor = 0.2f;
    
//     WaitTime = 0;
//     WaitTimeNum = 0;
//     WaitFlag = 0;
// }
void MotionPlan::ResetTracking()
{
    // 保存当前参数
    float saved_eta = dotTrackingParam.eta;
    float saved_switchlen = dotTrackingParam.switchlen;
    float saved_maxSpeed = dotTrackingParam.maxSpeed;
    float saved_setVel = dotTrackingParam.setVel;
    float saved_minSpeedFactor = dotTrackingParam.minSpeedFactor;
    
    // 重置结构体
    memset(&dotTrackingParam, 0, sizeof(TrackingPlanParam));
    
    // 恢复重要参数
    dotTrackingParam.eta = (saved_eta > 0) ? saved_eta : 0.8f;
    dotTrackingParam.switchlen = (saved_switchlen > 0) ? saved_switchlen : 15.0f;
    dotTrackingParam.maxSpeed = (saved_maxSpeed > 0) ? saved_maxSpeed : 5.0f;
    dotTrackingParam.setVel = (saved_setVel > 0) ? saved_setVel : 2.0f;
    dotTrackingParam.minSpeedFactor = (saved_minSpeedFactor > 0) ? saved_minSpeedFactor : 0.2f;
    
    // 重置其他状态
    dotTrackingParam.TrackNum = 0;
    
    WaitTime = 0;
    WaitTimeNum = 0;
    WaitFlag = 0;
}

bool MotionPlan::IsTrackingComplete() const
{
    return (dotTrackingParam.ReturnFlag == 1);
}

float MotionPlan::GetDistanceToTarget(const TransState& boatstate) const
{
    return static_cast<float>(calculateGeoDistance(
        boatstate.PosEarth[0], boatstate.PosEarth[1],
        dotTrackingParam.TargetPos[0], dotTrackingParam.TargetPos[1]
    ));
}

float MotionPlan::GetCurrentTargetSpeed() const
{
    return dotTrackingParam.setVel;
}

int MotionPlan::GetCurrentWaypointIndex() const
{
    return dotTrackingParam.TrackNum;
}

int MotionPlan::GetTotalWaypoints() const
{
    return dotTrackingParam.DotNum;
}

// 私有辅助函数实现
double MotionPlan::calculateGeoDistance(double lat1, double lon1, double lat2, double lon2) const
{
    double dlat = deg2rad(lat2 - lat1);
    double dlon = deg2rad(lon2 - lon1);
    double a = sin(dlat/2) * sin(dlat/2) + cos(deg2rad(lat1)) * cos(deg2rad(lat2)) * sin(dlon/2) * sin(dlon/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    return EARTH_RADIUS * c;
}

double MotionPlan::calculateGeoBearing(double lat1, double lon1, double lat2, double lon2) const
{
    double dlon = deg2rad(lon2 - lon1);
    double lat1_rad = deg2rad(lat1);
    double lat2_rad = deg2rad(lat2);
    
    double y = sin(dlon) * cos(lat2_rad);
    double x = cos(lat1_rad) * sin(lat2_rad) - sin(lat1_rad) * cos(lat2_rad) * cos(dlon);
    
    double bearing = rad2deg(atan2(y, x));
    return fmod(bearing + 360.0, 360.0); // 归一化到[0, 360)
}

double MotionPlan::normalizeAngle(double angle) const
{
    while (angle > 180.0) angle -= 360.0;
    while (angle < -180.0) angle += 360.0;
    return angle;
}